//
//  action_ctrl.c
//  smart_garbage_case
//
//  Created by fukai,caihui on 2018/11.
//  Copyright © 2018年 Cyber01. All rights reserved.
//

#include "pwm.h"
#include "action_ctrl.h"


//动作控制结构体初始化
ActionCtrlNode ActionCtrl =
{
	//舵机控制部分
	.ServoCtrl.pwm_close = 2500,
	.ServoCtrl.pwm_open = 4500,
	.ServoCtrl.ServoAcFlag = COVER_CLOSE,
	
	//电机控制部分
	.MotorCtrl.front_left_pwm = 500,
	.MotorCtrl.front_right_pwm = 500,
	.MotorCtrl.rear_left_pwm = 500,
	.MotorCtrl.rear_right_pwm = 500,
	.MotorCtrl.MotorACFlag = CAR_STOP,
	
	//速度控制
	.MecanumWheelParm.car_length = 3,
	.MecanumWheelParm.car_width = 3,
	.MecanumWheelParm.WheelEV.wheel1_ev = 0,
	.MecanumWheelParm.WheelEV.wheel2_ev = 0,
	.MecanumWheelParm.WheelEV.wheel3_ev = 0,
	.MecanumWheelParm.WheelEV.wheel4_ev = 0,
	.MecanumWheelParm.CarEV.x_axis_ev = 0,
	.MecanumWheelParm.CarEV.y_axis_ev = 0,
	.MecanumWheelParm.CarEV.z_axis_ew = 0,
};


//舵机状态转变
void ServoStatusChange(void)
{
	ActionCtrl.ServoCtrl.ServoAcFlag++;
	if(ActionCtrl.ServoCtrl.ServoAcFlag > COVER_OPEN)
	{
		ActionCtrl.ServoCtrl.ServoAcFlag = COVER_CLOSE;
	}
}

//电机状态转变
//开始or停止
void MotorStatusChange(void)
{
	ActionCtrl.MotorCtrl.MotorACFlag++;
	if(ActionCtrl.MotorCtrl.MotorACFlag > CAR_START)
	{
		ActionCtrl.MotorCtrl.MotorACFlag = CAR_STOP;
	}
}

//舵机状态控制
void ServoStatusCtrl(void)
{
	switch(ActionCtrl.ServoCtrl.ServoAcFlag)
	{
		case COVER_CLOSE:
			ServoPwnSet(ActionCtrl.ServoCtrl.pwm_close); 
			break;
		case COVER_OPEN:
			ServoPwnSet(ActionCtrl.ServoCtrl.pwm_open); 
			break;
	}
}



//获取期望速度矢量
//用于中断中
int y_times = 3, x_times = 3, z_times = 1;
int turn_speed = 80;
void GetExpectSpeedVectorTimer(int times)
{
	static int t = 0;
	t++;
	
	if(t > 0 && t <= y_times*times)
	{
		ActionCtrl.MecanumWheelParm.CarEV.x_axis_ev = 0;
		ActionCtrl.MecanumWheelParm.CarEV.y_axis_ev = 300;
		ActionCtrl.MecanumWheelParm.CarEV.z_axis_ew = 0;
	}
	else if(t > y_times*times && t <= y_times*times+x_times*times)
	{
		ActionCtrl.MecanumWheelParm.CarEV.x_axis_ev = -300;
		ActionCtrl.MecanumWheelParm.CarEV.y_axis_ev = 0;
		ActionCtrl.MecanumWheelParm.CarEV.z_axis_ew = 0;
	}
	else if(t > y_times*times+x_times*times && t <= y_times*times+x_times*times+z_times*times)
	{
		ActionCtrl.MecanumWheelParm.CarEV.x_axis_ev = 0;
		ActionCtrl.MecanumWheelParm.CarEV.y_axis_ev = 0;
		ActionCtrl.MecanumWheelParm.CarEV.z_axis_ew = turn_speed;
	}
	else if(t > y_times*times+x_times*times+z_times*times)
	{
		ActionCtrl.MotorCtrl.MotorACFlag = CAR_STOP;
		t = 0;
	}
}



//电机控制
void MotorCtrl(void)
{
	switch(ActionCtrl.MotorCtrl.MotorACFlag)
	{
		case CAR_STOP:
			InverseKinematicsAnalysis(&ActionCtrl.MecanumWheelParm);
			ActionCtrl.MotorCtrl.front_left_pwm = 500;
			ActionCtrl.MotorCtrl.front_right_pwm = 500;
			ActionCtrl.MotorCtrl.rear_left_pwm = 500;
			ActionCtrl.MotorCtrl.rear_right_pwm = 500;
			break;
		case CAR_START:
			GetExpectSpeedVectorTimer(333);
			InverseKinematicsAnalysis(&ActionCtrl.MecanumWheelParm);
			ActionCtrl.MotorCtrl.front_left_pwm = 500+ActionCtrl.MecanumWheelParm.WheelEV.wheel2_ev;
			ActionCtrl.MotorCtrl.front_right_pwm = 500+ActionCtrl.MecanumWheelParm.WheelEV.wheel1_ev;
			ActionCtrl.MotorCtrl.rear_left_pwm = 500+ActionCtrl.MecanumWheelParm.WheelEV.wheel3_ev;
			ActionCtrl.MotorCtrl.rear_right_pwm = 500+ActionCtrl.MecanumWheelParm.WheelEV.wheel4_ev;
			break;
	}
	
	
	
	//设置电机占空比
	FrontLeftPwmSet(ActionCtrl.MotorCtrl.front_left_pwm);
	FrontRightPwmSet(ActionCtrl.MotorCtrl.front_right_pwm);
	RearLeftPwmSet(ActionCtrl.MotorCtrl.rear_left_pwm);
	RearRightPwmSet(ActionCtrl.MotorCtrl.rear_right_pwm);
}








